#include <iostream>
#include <iostream>
#include <iomanip>



using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;
#include "UUV.h"
#include"DrawTrajectory.h"
#include "controller.h"
int main()
{
    double Te = 400;
    Matrix<double, 6, 2> state_0;
    state_0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
    UUV *uuv = new UUV(state_0);
    controller *controll = new controller(uuv->getstate());
    while(uuv->getT() < Te){

        uuv->update(controll->getTau());
        controll->update(uuv->getstate(), uuv->getF());
        //uuv->DrawTrajectory();

    }
    cout << uuv->getT()<< endl << uuv->getstate() << endl;

    //DrawTrajectory(controll->getState_d());
    DrawTrajectory(uuv->getState(),controll->getState_d());

    return 0;
}
